package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.EntranceLocatorState;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IMazeSolver;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IRobotSensors;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.UnloadMazeNavigatorState;

/**
 * This class provides the robot for navigation in the maze during
 * unload. The following actions will be performed:
 * <ul>
 * <li>Enters into the maze.
 * <li>Jogs in the maze.
 * <li>Detects the drop zone.
 * <li>Calls the treasure unloader to unload treasure.
 * <li>Calls the turner to turn when there is a junction.
 * <li>Exits the Maze.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

enum UnloadStage {
	UNLOADING,
	TREASURE_FOUND,
	EXITING,
	EXIT_FOUND,
}

public class UnloadMazeNavigator extends StateObject {
	
	private RobotCommandSequencer m_commandSequencer;
	private IMazeSolver m_solver;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private Map<Integer, IDetector> m_colorDetectors;
	private IRobotSensors m_sensors;
	private RobotCommand m_command;
	private TreasureUnloader m_treasureUnloader;
	private UnloadStage m_unloadStage;
	private static int MOVE_OUT_OF_MAZE_DISTANCE = 200; //mm
	private static int MOVE_INDEX = 100; //mm
	private static int ROTATE_INDEX = 10; //degree
	private int m_rotateAngle = 0;

	public UnloadMazeNavigator(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_solver = robot.m_solver;
		m_positionDetector = robot.m_positionDetector;
		m_colorDetectors = robot.m_colorDetectors;
		m_sensors = robot.m_sensors;
		m_command = new RobotCommand();
		m_treasureUnloader = new TreasureUnloader(robot);
	}
	
	@Override
	public void Initialize() {
		//m_treasureUnloader.Initialize();
		//m_commandSequencer.Initialize();
		machineState = UnloadMazeNavigatorState.START;
		machineRecoveryState = machineState;
		m_unloadStage = UnloadStage.UNLOADING;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == UnloadMazeNavigatorState.DONE;
	}

	public IStateMachine checkBeforeJog() {

		m_robotPosition = m_positionDetector.getPosition();
		
		boolean needRotate = false;
		
		if (m_robotPosition.isTooLeft() || m_robotPosition.isLeft()) {
			int[] values = m_sensors.getLeftDistances();
	
			if (m_robotPosition.isTooLeft() || (values[0] > values[1] + 500)) {
				m_rotateAngle = ROTATE_INDEX;
				needRotate = true;
			}
		}
		else if (m_robotPosition.isTooRight() || m_robotPosition.isRight()) {
			int[] values = m_sensors.getRightDistances();
			
			if (m_robotPosition.isTooRight() || (values[0] > values[1] + 500)) {
				m_rotateAngle = -ROTATE_INDEX;
				needRotate = true;
			}
		}

		if (needRotate) {
			machineState = UnloadMazeNavigatorState.ROTATE_BEFORE_START_JOG;
		}
		else {
			machineState = UnloadMazeNavigatorState.START_JOG;
		}
		return machineState;
	}
	
	public IStateMachine rotateBeforeJog() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_rotateAngle);

		if (returnVal == ReturnResult.Completed) {
			log("startJog.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.CHECK_BEFORE_START_JOG;
		}
		
		return machineState;
	}
	
	public IStateMachine startJog() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 4);

		if (returnVal == ReturnResult.Completed) {
			log("startJog.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.SCAN;
		}
		
		return machineState;
	}
	
	public IStateMachine scan() {

		if (m_unloadStage == UnloadStage.EXITING && 
			m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
			m_unloadStage = UnloadStage.EXIT_FOUND;
			log("exit found.UnloadMazeNavigator", 4);
		}
		
		if (m_unloadStage == UnloadStage.UNLOADING && 
			m_colorDetectors.get(DetectionIndex.DROPZONE).isPresent()) {
			log("detectDropZone = true.UnloadMazeNavigator", 4);
			
			machineRecoveryState = UnloadMazeNavigatorState.UNLOAD_TREASURE;
			machineState = UnloadMazeNavigatorState.STOP_JOG;
		}
		else {
			CheckJunction();
		}
		
		return machineState;
	}
	
	private void CheckJunction() {
		if (m_sensors.isWheelMoving())
			return;
		
		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.leftwalldetected || m_robotPosition.rightwalldetected)
			machineState = UnloadMazeNavigatorState.OVER_JOG_BEFORE_TURN;
		else if (m_robotPosition.frontwalldetected)
			machineState = UnloadMazeNavigatorState.DETERMINE_NEXT_ACTION;
	}
	
	public IStateMachine determineNextAction() {
		m_robotPosition = m_positionDetector.getPosition();
		m_command = m_solver.nextAction(m_robotPosition, false);
		
		switch (m_command.action) {
		case Jog:
			log("Jog.UnloadMazeNavigator", 4);
			if (m_unloadStage == UnloadStage.EXIT_FOUND) {
				machineState = UnloadMazeNavigatorState.MOVE_OUT_OF_MAZE;
			}
			else if (m_sensors.isArmUp()) {
				log("robot auto stopped.UnloadMazeNavigator", 4);
				machineState = UnloadMazeNavigatorState.CHECK_BEFORE_START_JOG;
			}
			else {
				log("ensureArmUp = false.UnloadMazeNavigator", 4);
				machineRecoveryState = UnloadMazeNavigatorState.MOVE_ARM_UP;
				machineState = UnloadMazeNavigatorState.STOP_JOG;
			}
			
			m_solver.onJunctionPassed(m_command.action);
			break;
		case TurnRight:
		case TurnLeft:
			if (m_unloadStage == UnloadStage.TREASURE_FOUND)
				m_unloadStage = UnloadStage.UNLOADING;
			
			if (m_unloadStage == UnloadStage.EXIT_FOUND) {
				machineRecoveryState = UnloadMazeNavigatorState.MOVE_OUT_OF_MAZE;
			}
			else {
				machineRecoveryState = UnloadMazeNavigatorState.TURN;
			}
			
			m_solver.onJunctionPassed(m_command.action);
			machineState = UnloadMazeNavigatorState.STOP_JOG;
			break;
		case TurnBack:
			if (m_unloadStage == UnloadStage.TREASURE_FOUND) {
				machineRecoveryState = UnloadMazeNavigatorState.UNLOAD_TREASURE;
			}
			else {
				machineRecoveryState = UnloadMazeNavigatorState.TURN;
			}
			
			m_solver.onJunctionPassed(m_command.action);
			machineState = UnloadMazeNavigatorState.STOP_JOG;
			break;
		default:
			assert(false);
			break;
		}
		
		return machineState;
	}
	
	public IStateMachine stopJog() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);

		if (returnVal == ReturnResult.Completed) {
			log("stopJog.UnloadMazeNavigator", 4);
			machineState = machineRecoveryState;
		}
		
		return machineState;
	}
	
	public IStateMachine moveArmUp() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.PickTreasure);

		if (returnVal == ReturnResult.Completed) {
			log("moveArmUp.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.CHECK_BEFORE_START_JOG;
		}
		
		return machineState;
	}
	
	public IStateMachine overJogBeforeTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 5);

		if (returnVal == ReturnResult.Completed) {
			log("overJogBeforeTurn.UnloadMazeNavigator", 4);
			sleep(1000);
			machineState = UnloadMazeNavigatorState.CHECK_OVER_JOG_BEFORE_TURN_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkOverJogBeforeTurnDone() {

		if (!m_sensors.isWheelMoving()) {
			log("checkOverJogBeforeTurnDone.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.DETERMINE_NEXT_ACTION;
		}

		return machineState;
	}
	
	public IStateMachine turn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(m_command.action, m_command.param1);

		if (returnVal == ReturnResult.Completed) {
			if (m_unloadStage == UnloadStage.EXITING && 
				m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
				m_unloadStage = UnloadStage.EXIT_FOUND;
				log("exit found.UnloadMazeNavigator", 4);
			}
			else if (m_unloadStage == UnloadStage.UNLOADING && 
				m_colorDetectors.get(DetectionIndex.DROPZONE).isPresent()) {
				m_unloadStage = UnloadStage.TREASURE_FOUND;
				log("treasure found.UnloadMazeNavigator", 4);
			}
			
			log("turn done.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.MOVE_TO_CHECK_IF_WALL_HIT_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine moveToCheckIfWallHitAfterTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveToCheckIfWallHitAfterTurn.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.CHECK_IF_WALL_HIT_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine checkIfWallHitAfterTurn() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.front1walldetected) {
			log("checkIfWallHitAfterTurn(LeftWallHit).UnloadMazeNavigator", 4);
			m_rotateAngle = ROTATE_INDEX;
			machineState = UnloadMazeNavigatorState.MOVE_BACK_DUE_TO_WALL_HIT_AFTER_TURN;
		}
		else if (m_robotPosition.front3walldetected) {
			log("checkIfWallHitAfterTurn(RightWallHit).UnloadMazeNavigator", 4);
			m_rotateAngle = -ROTATE_INDEX;
			machineState = UnloadMazeNavigatorState.MOVE_BACK_DUE_TO_WALL_HIT_AFTER_TURN;
		}
		else {
			log("checkIfWallHitAfterTurn(not wall hit).UnloadMazeNavigator", 4);
			if (m_unloadStage == UnloadStage.EXITING && 
				m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
				m_unloadStage = UnloadStage.EXIT_FOUND;
				log("exit found.UnloadMazeNavigator", 4);
			}
			
			if (m_unloadStage == UnloadStage.EXIT_FOUND) {
				machineState = UnloadMazeNavigatorState.MOVE_OUT_OF_MAZE;
			}
			else {
				machineState = UnloadMazeNavigatorState.CHECK_BEFORE_START_JOG;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine moveBackDueToWallHitAfterTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveBackward2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveBackDueToWallHitAfterTurn.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.ROTATE_TO_AVOID_WALL_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine rotateToAvoidWallAfterTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, m_rotateAngle);

		if (returnVal == ReturnResult.Completed) {
			if (m_unloadStage == UnloadStage.EXITING && 
				m_colorDetectors.get(DetectionIndex.MAZE_EXIT).isPresent()) {
				m_unloadStage = UnloadStage.EXIT_FOUND;
				log("exit found.UnloadMazeNavigator", 4);
			}
			log("moveToCheckIfWallHitAfterTurn.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.MOVE_TO_CHECK_IF_WALL_HIT_AFTER_TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine unloadTreasure() {
		
		ReturnResult returnVal = m_treasureUnloader.PerformAction();

		if (returnVal == ReturnResult.Completed) {
			log("unloadTreasure.UnloadMazeNavigator", 4);
			m_unloadStage = UnloadStage.EXITING;
			machineState = UnloadMazeNavigatorState.CHECK_BEFORE_START_JOG;
		}
		
		return machineState;
	}
	
	public IStateMachine moveOutOfMaze() {
		
		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_OUT_OF_MAZE_DISTANCE);

		if (returnVal == ReturnResult.Completed) {
			log("moveOutOfMaze.UnloadMazeNavigator", 4);
			machineState = UnloadMazeNavigatorState.DONE;
		}
		
		return machineState;
	}

}